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Abstract — The paper presents a two-stage approach for de- 
signing optimal reconfiguration maneuvers for multiple space- 
craft. These maneuvers involve well-coordinated and highly- 
coupled motions of the entire fleet of spacecraft while sat- 
isfying an arbitrary number of constraints. This problem is 
particularly difficult because of the nonlinearity of the attitude 
dynamics, the non-convexity of some of the constraints, and the 
coupling between the positions and attitudes of all spacecraft. 
As a result, the trajectory design must be solved as a single 6N 
DOF problem instead of N separate 6 DOF problems. The first 
stage of the solution approach quickly provides a feasible initial 
solution by solving a simplified version without differential 
constraints using a bi-directional Rapidly-exploring Random 
Tree (RRT) planner. A transition algorithm then augments this 
guess with feasible dynamics that are propagated from the 
beginning to the end of the trajectory. The resulting output 
is a feasible initial guess to the complete optimal control 
problem that is discretized in the second stage using a Gauss 
pseudospectral method (GPM) and solved using an off-the- 
shelf nonlinear solver. This paper also places emphasis on the 
importance of the initialization step in pseudospectral methods 
in order to decrease their computation times and enable the 
solution of a more complex class of problems. Several examples 
are presented and discussed. 

I. INTRODUCTION 

The Terrestrial Planet finder (TPF) [1], the Laser Inter- 
ferometer Space Antenna Project (LISA) [2], the Micro- 
Arcsecond X-ray Imaging Mission (MAXIM) [3], the Sys- 
tem F6 Program to demonstrate a fractionated spacecraft 
approach [4], as well as many other future space missions 
and programs will be enabled by a formation flying technol- 
ogy for multiple spacecraft. Formation flying of spacecraft 
consists of more than one spacecraft whose dynamical states 
are coupled through a common control law [5]. For example, 
the proposed TPF observatory consists of multiple spacecraft 
carrying infrared telescopes [6], [1]. The vehicles are inde- 
pendent, but they are coupled through the control objective 
of achieving a precise telescope. 

Formation flying has been extensively investigated as a 
means to expand the capabilities of space missions focused 
on obtaining magnetosphere and radiation measurements, 
gravity field measurements, and 3-D mapping for planetary 
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explorers (to name a few). The use of fleets of small 
satellites, instead of a single monolithic satellite, enables 
higher resolution imagery and interferometry, robust and 
redundant fault-tolerant spacecraft system architectures, and 
more complex networks of satellites, thereby improving 
science return. To achieve these benefits, tighter requirements 
will be imposed on the communication and coordination 
between spacecraft, path planning algorithms, autonomous 
fault detection and recovery, and on the high level mission 
management [7]. 

There are two key types of trajectory design problems for 
formation flying spacecraft: 1) reconfiguration, which con- 
sists of maneuvering a fleet of spacecraft from one formation 
to another, and 2) station-keeping, which consists of keeping 
a cluster of fleet of spacecraft in a specific formation for a 
determined part of the trajectory. Both types of formation 
flying maneuvers must be addressed for deep-space missions 
where the relative spacecraft dynamics usually reduces to 
double integrators, or planetary orbital environment flying 
missions where spacecraft are subjected to significant orbital 
dynamics and environmental disturbances [8]. 

This paper focuses on the trajectory design of recon- 
figuration maneuvers of multiple spacecraft in deep space 
environment. They consist of moving and rotating a group 
of N spacecraft from an initial configuration to a desired final 
configuration, while satisfying different types of constraints 
(see Figure 1). These constraints may consist of collision 
avoidance, restrictions on the region of the sky where certain 
spacecraft instruments can point (e.g., a sensitive instrument 
that cannot point at the Sun), or restrictions on pointing 
towards other spacecraft (e.g., requirements on maintaining 
inter- spacecraft communication links and having cold science 
instruments avoid high temperature components on other 
vehicles). 

It is also desirable to optimize some performance index 
(fuel, energy, maneuver time, etc.) [8]. This problem is par- 
ticularly difficult because of the nonlinearity of the attitude 
dynamics, the non-convexity of some of the constraints, and 
the coupling between the positions and attitudes of all space- 
craft. Even though several solutions exist for the attitude 
control problem alone, its intrinsic complexity, arising from 
its nonlinearity, makes the general spacecraft reconfigura- 
tion problem harder. The non-convex constraints place this 
problem in a general class of path planning problems with a 
computational complexity that is exponential in the number 
of degrees of freedom of the problem. In addition, some 
types of pointing constraints force coupling between the 
position and attitude the spacecraft, making it impossible 
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Fig. 1. The formation reconfiguration problem [9] 

to separate the translation control problem from the attitude 
control problem. As a result, the trajectory design must be 
solved as a single 6N DOF problem instead of N separate 
6 DOF problems. Since the size of future formation flight 
missions will continue to increase [10], new path planning 
techniques should be able to handle large scale formations. 
Another requirement is that these planning algorithms should 
have fast computation times suitable for an eventual online 
implementation. 

II. Survey of Previous Work 

The spacecraft trajectory design of the constrained and un- 
constrained translation and attitude maneuvers has been the 
subject of extensive research in formation flying spacecraft. 
Most of the solutions for this problem consider either the 
translation or the attitude trajectories. Refs. [11], [12] pro- 
posed the use of Mixed Integer Linear Programming (MILP) 
or Mixed Integer Linear Matrix Inequalities (MI/LMI) tech- 
niques to solve the problem. These methods require several 
simplifications in formulating the problem. MILP deals with 
linear problems, therefore the systems dynamics as well as 
the constraints should be represented in linear form. A major 
drawback of the MI/LMI solution technique is that the size 
of the problem increases dramatically with the number of 
spacecraft, whereas solving a MILP problem usually requires 
branch and bound techniques. Some authors considered the 
use of potential functions in the solution of path planning 
problems [13], [14]. The major drawback of this type of 
methods is that the trajectory it generates might get trapped 
in local minima. Moreover, computing a potential function 
that is free of local minima is computationally very hard for 
any non-trivial set of constraints [15]. This approach cannot 
either guarantee that the resulting trajectories are collision 
free, which is critical in spacecraft formation flying missions. 

Another popular approach that has been investigated re- 
cently with great success in motion planning research is 
the use of randomized motion planning algorithms such as 
the probabilistic roadmap (PRM) planners [16], [17], and 
their incremental counterparts the Rapidly-exploring Ran- 


dom Trees (RRT) algorithms. But the application of RRTs on 
spacecraft reconfiguration problems was limited to 1) a prob- 
lem involving a single spacecraft with no pointing constraints 
[18], 2) a problem considering the attitude maneuver of one 
spacecraft [15], and 3) a multi-spacecraft reconfiguration 
problem that solves for the translation trajectory only [19]. 
It’s only very recently that the more general case of combined 
translation and attitude reconfiguration of multiple spacecraft 
problems has been addressed using RRTs [20], [21]. This ap- 
proach consists of a two- stage planning algorithm, similar to 
the one developed in this research. However, its second stage, 
also called the “smoothing” step, is based on linearizing a 
nonlinear optimization problem around the feasible solution 
generated by the first stage. This induces linearization errors 
in the solution of the problem, which can make it infeasible. 
Additional work is needed to restore feasibility, and it is 
problem dependent. 

Numerous researchers have recently explored using pseu- 
dospectral methods for nonlinear trajectory optimization 
problems related to aerospace applications [22], [23], [24]. 
One major negative aspect of pseudospectral methods in 
general is that the computation time increases dramatically 
with the complexity of the problem. There are many possible 
reasons for this increase, but one noticeable issue is that 
simply finding a feasible solution to a problem as complex as 
the multi- spacecraft reconfiguration problem set of solutions. 
Providing a feasible initial guess to the solver should help 
decrease this computation time, but this is complex since 
the path planning problem with general constraints is NP- 
hard [25]. Ref. [23] suggests using a “warm start” to im- 
prove the computation times of the problem. A warm start 
considers the solution of previous optimizations as an initial 
guess to the current problem. This idea is similar to the 
mesh refinement technique introduced in Ref. [26], which 
starts with a coarse grid (i.e. low number of discretization 
nodes), and if necessary, refines the discretization, and then 
repeats the optimization steps. But these approaches can 
be very time consuming, and therefore not feasible for 
online planning of reconfiguration maneuvers. If a warm start 
process is to be efficient, the algorithm must be chosen with 
care. The technique presented in this paper improves the 
performance of pseudospectral method based problems by 
providing a feasible initial guess. This guess is the solution 
of a simplified version of the path planning problem without 
differential constraints. This problem is quickly solved using 
an improved version of bidirectional RRTs [21]. 

III. Solution Concepts 

This paper presents a two-stage path planning algorithm to 
solve the problem described in Section IV. The first stage, 
discussed in Section III-B, is based on Rapidly-exploring 
Random Trees (RRTs), a randomized planning technique that 
has been very popular recently. Then, Section III-C discusses 
the use of pseudospectral methods as a solution technique for 
the optimal control problem formed in the second stage of the 
path planning algorithm. This two-stage technique extends 
the original ideas in Ref. [9] to improve the second step 


by using a specific pseudospectral method, called the Gauss 
pseudospectral method (GPM). 

A. Two-Stage Path Planning 

The constraints encountered in spacecraft reconfiguration 
maneuver problems fall into two main categories, (a) kine- 
matic and (b) dynamic. Kinematic constraints address the 
motion of the spacecraft under consideration, but ignore the 
forces behind the motion, which are captured in the dynamic 
constraints. Path planning for reconfiguration maneuvers 
is a challenging task even when considering each set of 
constraints individually. When addressing these two types of 
constraints simultaneously, the problem is known as kinody- 
namic motion planning [27], which has been traditionally 
implemented using two common approaches: “two-stage” 
planning and “state-space” formulation [28], [27]. Unlike 
the “state-space” approach, where the dynamic constraints 
are taken into account from the start of the algorithm [29], 
[30], the “two-stage” formulation consists of first finding a 
feasible path that satisfies the kinematic constraints, and then 
optimizing this path to include the dynamic constraints [31], 
[20]. This paper develops a two-stage approach for solving 
reconfiguration maneuvers of multiple spacecraft. Examples 
of complex maneuvers including up to five spacecraft illus- 
trate this approach. 

B. Rapidly Exploring Random Trees 

The first stage of the two- stage algorithm developed in this 
paper concentrates on finding any feasible trajectory for the 
problem, postponing the “smoothing” or cost improvement 
to the second stage. However, finding a feasible path with 
guarantees is by itself very difficult because the path planning 
problem becomes intractable for high dimensional problems 
like the multiple spacecraft reconfiguration maneuver prob- 
lem. But it has been shown that if the guaranteed completion 
is relaxed, larger problems can be solved using randomized 
path planning algorithms, such as the Probabilistic Roadmaps 
(PRMs) [16]. Rapidly exploring Random Trees (RRTs), a 
recent variant of PRMs introduced in Refs. [32], [33], was 
developed for planning under differential constraints, but 
it has been applied mostly in ordinary motion planning. 
The RRT structure and algorithm are designed to efficiently 
explore high-dimensional spaces, therefore quickly finding a 
feasible solution even in highly constrained environments. 

RRTs have several nice properties [32]. We emphasize 
two of them: 1) their expansion is heavily biased towards 
unexplored areas of the configuration space ( e.g ., see Fig- 
ure 2) and 2) the RRT algorithm is probabilistically complete 
i.e. f the probability of finding a feasible path approaches one 
as the number of iterations increases. RRTs and their variants 
have been applied successfully in several applications in dif- 
ferent areas of research including robotics and graphics [34] . 
This paper uses an improved version of the well known 
bidirectional RRTs, a technique that has been introduced and 
shown to be a very fast planner for trajectory optimization 
problems when differential constraints are ignored [21]. 
Section V-A describes this method in more detail. 



Fig. 2. Example of RRT expansion starting from center of square [32] 
C. Pseudospectral Methods 

The second stage of the planning algorithm developed in 
this paper is formulated as an optimal control problem with 
path constraints. Numerical methods for solving this type of 
problems fall into two general categories: direct methods and 
indirect methods [35]. 

In an indirect method, the optimal solution is found by 
solving a Hamiltonian boundary-value problem derived from 
the first-order necessary conditions for optimality. The pri- 
mary advantages of indirect methods are their high accuracy 
in the solution and the assurance that the solution satisfies the 
first-order optimality conditions. However, indirect methods 
have several disadvantages including possible difficulties 
in deriving the Hamiltonian boundary-value problem, small 
radii of convergence, and the requisite of a good initial guess 
for both the state and costate. 

In a direct method, the continuous-time optimal control 
problem is transcribed to a nonlinear programming problem 
(NLP). The resulting NLP can be solved by well developed 
algorithms and software. Direct methods have the advantage 
that the optimality conditions do not need to be derived. They 
do suffer however, depending on the type of direct method, 
in that the solution may not contain any costate information, 
or may result in an inaccurate costate. 

As the number of spacecraft in the reconfiguration problem 
increases, solving the Hamiltonian boundary value problem 
becomes increasingly difficult, if not impossible. Moreover, 
advances in direct methods, such as the pseudospectral 
methods [36], [37], have improved the accuracy of the costate 
information compared to earlier direct methods. 

The states and controls in pseudospectral methods are 
parameterized using a basis of global polynomials which are 
derived from an appropriate set of discretization points [38]. 
The use of global orthogonality makes it simple to transform 
the original problem into a set of algebraic equations. The 
discretized optimal control problem is then transcribed to 
a nonlinear program which can then be solved using an 
off-the-shelf nonlinear solver. This paper uses the Gauss 
pseudospectral method (GPM), one of the newest numerical 
approaches in the literature today, that has shown promise 
both in the solution and in the post-analysis optimality [22], 
[39]. Section V-C describes the Gauss pseudospectral method 
in its most current form. 

IV. Problem Formulation 

The general reconfiguration problem resides in finding a 
trajectory of N spacecraft from time 0 to time T . Let Pi(t) 


be a point of the trajectory of a single spacecraft at time t. 
This point consists of 

Pi(t) = [Xi(t),Ui(t)], (1) 

where a^(t) and Ui( t) represent the state and control inputs 
at each time £, respectively, 

Xi (t) = [Vi ( t ) , fi ( t ) , Wi (t) , CTi (t )] , (2) 

Ui(t) = [fi(t),Ti(t)], (3) 

and where i G 1 ...N indicates the spacecraft. r*( t) G M 3 is 
the position of its center, f^(t) G M 3 is its velocity, Wi( t) 
G M 3 its angular velocity, and cr^(t) G M 3 is its attitude 
representation in modified Rodrigues parameters (MRP) [40] . 
All these variables are measured with respect to a local 
inertially fixed frame. /i(t) G M 3 represents the control input 
force, and r^(t) G M 3 the control input torque. Therefore, 

p(t) = (4) 


represents a point in the composite trajectories of all the 
spacecraft at time t. Since the interest of this research 
is in deep space missions, the translation dynamics are 
approximated with a simple double integrator 
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where M G M is the mass, assumed to be the same for all 
spacecraft for simplicity. Is X 3 is the 3 x 3 identity matrix, 
and 03 x 3 is the 3 x 3 zero matrix. 

The attitude dynamics in MRP notation of the spacecraft 
considered as a rigid body are 


&i(t) = R(ai(t))wi(t) (6) 

JWi(t) = -Wi(t) X JWi(t ) + Ti(t) 

= + Ti(t) (7) 

where J G M 3 is the spacecraft constant inertia matrix, 
considered to the same for all spacecraft for simplicity. S 
G M 3x3 is the skew- symmetric matrix representing the cross 
product operation 


S(a) = [ax] = 


0 — a 3 a 2 

as 0 — a\ 

— a 2 a\ 0 


Va € R 3 (8) 


The Jacobian matrix R e R 3x3 for MRP attitude represen- 
tation is given by [40] 

R(<Ti) '1 [(1 - 3x3 + 2 S(<Ti) + 2 (Titrf] (9) 

The path constraints can be divided into two categories: 
1) collision avoidance constraints, and 2) pointing restriction 
constraints. 

The collision avoidance category contains the inter- 
spacecraft collision avoidance constraints, which ensure safe 
separation between every pair of spacecraft, and are written 


for i, j G 1. . . N, i ^ j, and Rij is the minimum distance 
allowed between the centers of spacecraft i and j. Collision 
avoidance also contains the obstacle avoidance constraints, 
which ensure safe maneuvering of every spacecraft among 
all obstacles, and are written as 


\\ri(t)nio(t)\\>Rio (ii) 

for every obstacle o, and for i G 1. . . N. l Q is the position of 
the center of obstacle o, and Ri 0 is the minimum distance 
allowed between the centers of spacecraft i and obstacle o. 

The pointing restriction category contains four types of 
constraints: 

• Absolute stay outside constraints 

• Absolute stay inside constraints 

• Relative stay outside constraints 

• Relative stay inside constraints 

The absolute stay outside constraints can be written as 

ZkVk{t)< cos0 k (12) 


for every stay outside pointing constraint k. This constraint 
ensures that the spacecraft vector y remains at an angle 
greater than 0k G [0, 7r] from the inertial vector Zk- The 
vector yk represents the body vector ykB in the inertial 
coordinate frame. The transformation of coordinates is given 
by 

Vk(t) = Rot~ l (a(t))y kB (13) 


where Rot(cr(t )) is the rotation matrix representation of the 
MRP attitude vector cr it ), which can be written as [40] 


Rot(a) = I + + ^1 %) 
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(14) 


where S is the matrix defined in (8). It is assumed that Pkb 
and Zk are fixed vectors i.e., independent of time t. 

The absolute stay inside constraints only change the sign 
of the inequality of (12). They can be written as 

zlyk(t)> cos#fc (15) 


The inter- spacecraft relative stay outside constraints are 
given by 

< cos8 k (16) 


where ^(t) and 0k are the same as defined above, and 


Vi jit) 




(17) 


represents the unit vector pointing from spacecraft i to 
spacecraft j. The inter- spacecraft relative stay inside can be 
similarly written as 


> cos 0 k (18) 


The boundary conditions specify the initial and final con- 
figuration i.e., state of each spacecraft. They can be written 
as 


Xi ( 0 ) — Xis 

Xi(T) Xif 


as 


\\ri(t) -rj(t)\\ > Rij 


( 10 ) 


(19) 

( 20 ) 



where Xi s represents the state corresponding to the specified 
starting condition, and Xif to the specified final condition 

Vi e 1 ... N. 

The state and control vectors are restricted to lie within 
specified bounds 

•Emin — — & max ( 21 ) 

'U'min — V U max ( 22 ) 

where the inequality is understood to be component wise. 
The bounds on the input control vectors are usually due to the 
limited thrust of each spacecraft. The bounds on the velocity 
vectors are usually characteristic of safety limits. Finally, the 
position bounds ensure that the problem space is bounded 

[41]. 

The objective is to minimize the total energy of the 
formation 

N r T 

J = E/ ll/^)U 2 + IMi)|| 2 di (23) 

i=l 

Minimizing the total energy consumption of a formation of 
spacecraft is an objective for many space missions [42], [43]. 
Furthermore, the energy is in general directly related to the 
fuel consumption. So minimizing energy energy leads to less 
fuel consumption. 

V. Solution Approach 
A. The RRT First Stage 

The first stage of the RRT-GPM algorithm is based on 
the improved version of the bidirectional rapidly-exploring 
random trees (RRT) developed by the authors of Ref. [21]. 
It is reproduced here for clarity. The original RRT algorithm 
was developed by Lavalle [34]. 


Algorithm 1 RRT-BIDECTIONAL (p i9 p f ) 

1: T a .init(pi); T b .init(p f ); 

2: for j <— 1 to K do 

3: p n <- Nearest (T a ,a(j)) 

4: p s <— POTENTIAL-CONNECT(p„,a(j)) 

5: if p s ± p n then 

6: T a . Add- Vertex(p s ) 

7: T a .ADD-EDGE(p n , p s ) 

8: Pn <- Nearest (T 6 , Ps ) 

9: p s <— POTENTIAL-CONNECT(p n , p s ) 

10: if p s ^ p n then 

11: T6.ADD-VERTEX(p s ) 

12: T 6 .ADD-EDGE(p n , p s ) 

13: end if 

14: if ps = Ps then 

15: return Solution 

16: end if 

17: end if 

18: Swap (T a , T b ) 

19: end for 

20: return Failure 


In Algorithm 1, T a and T p represent trees having a 
composite trajectory point p at each node (4). T a starts from 
the initial point and T b starts from the final point of the 
goal trajectory. At each node, the points p are considered at 
rest, so the position and attitude are the only information of 
interest in this algorithm. At each iteration, a{i) generates a 
random point, and then the point in the tree T a with the 
minimum distance to the point a(i) is found by calling 
Nearest (T a , a{i)). Distance in this context represents a 
weighted summation of rotation and translation. It can be 
written as 

N 

d(pi,p 2 ) = ^ ||ri,j - r 2 ,i\\ + K a Z(q h i, q 2 ,i ) (24) 

where Z(qfi,i, represents the angle of an eigen-axis 
rotation between attitude q\ i and for spacecraft i, and 
K a is a weight factor that relates the translation distance and 
rotation angle. 

Potential-Connect is an artificial potential function 
based on a distance metric d(pi,P 2 ) where the obstacle 
avoidance, restricted pointing, and other constraints are 
represented by inequality and equality constraints [21]. So 
Potential- Connect is a search algorithm that tries to 
find a sequence of feasible points with a decreasing distance 
to the target point. This search can be formulated as a 
nonlinear optimization problem Algorithm 2. The solution 
to this problem can be found using a feasible sequential 
optimization method, and thus guarantees that the sequence 
of points represent a valid trajectory. 


Algorithm 2 POTENTIAL-CONNECT(p, pf) 
l: for j <— 1 to K do 
2: Solve nonlinear program: 

min dp d(p + dp,p f ) 
subject to 

9min,i — 9i{P V dp) ^ Qmax^i^'l 

\\ d P\\ < e 

► End of nonlinear program 
3: p <— p + dp 

4: end for 
5: return p 


Potential-Connect tries to connect p to pf by moving 
in small dp increments. These dp increments are restricted 
to be smaller in norm than e to guarantee feasibility between 
adjacent points of the trajectory. Note that the numerical 
experiments were done using a custom sequential linear 
solver that computes the solution of a sequence of linear 
programs with linearized constraints. 

So the solution of the first stage consists of a sequence 
of points from the initial point pi to the final point pf. At 
each point, the spacecraft are assumed to be at rest, and there 
exists a direct motion to the next point that is guaranteed to 
satisfy all the constraints. This improved bidirectional RRT 
planner has been demonstrated to be significantly faster than 
other similar spacecraft reconfiguration maneuver planners. 



For more details and illustrations of Algorithm 1 and Al- 
gorithm 2, the interested reader is encouraged to consult 
references [21] and [9]. 

B. The Augmentation with Feasible Dynamics 

A major simplification in the first stage of the RRT-GPM 
algorithm is based on ignoring the differential constraints of 
the spacecraft reconfiguration problem. This simplification is 
essential in decreasing the computation time of the first stage. 
The RRT solution of the first stage is clearly suboptimal since 
the spacecraft are assumed to be at rest at each of the nodes, 
and no cost function is actually optimized. Therefore, a 
second stage is needed to improve the cost of the trajectory. A 
transition step that augments the RRT solution with feasible 
dynamics is thus required to allow using this solution as 
a feasible initial guess to the second stage. So a main 
requirement of this transition step is to ensure that feasibility 
is maintained between the first and second stage. 

The idea of this transition step starts with adding an 
intermediate node p inter half way between every pair of 
nodes. First assume that the problem consists of only one 
spacecraft. To propagate the dynamics to p mter , the space- 
craft is assumed to accelerate under a constant input force 
f and a constant input torque f . f and f are chosen such 
that they satisfy the bounds on the forces and torques defined 
in (22). Once the spacecraft reaches p current 9 it decelerates 
under a constant force -/ and a constant torque -f until it 
stops at the next node p next . This is simple way to guarantee 
that the controls of the spacecraft are satisfied along each 
consecutive nodes. The smaller f and f are, the longer the 
total maneuver time is. Therefore, these values should be 
chosen to also satisfy the design specifications ( e.g ., total 
maneuver time) of the reconfiguration maneuver. Note that 
the restriction that the intermediate node lies exactly in 
between the original pair of nodes only exists in the initial 
guess. After the initial guess is given to the GPM stage, that 
restriction disappears, along with the assumption of fixed 
forces and torques. 

To expand this idea to multiple spacecraft, it is necessary 
that all spacecraft reach the intermediate node p inter at the 
same instant of time. This synchronization assumption is a 
simple way to ensure the feasibility of the algorithm when 
considering multiple spacecraft. But again, this assumption 
is relaxed after the guess is given to the second stage, i.e., the 
final solution of the RRT-GPM approach is not required to 
satisfy it. First of all, £ ma;r is computed. t max represents the 
maximum time needed by all spacecraft to reach p mter , if 
they all move under the same constant force / and rotate 
under the same constant torque f . Then fixing the time 
to reach p inter to be for all spacecraft, the constant 
forces and torques responsible to move each spacecraft are 
recomputed. Therefore some spacecraft will be designed to 
move under forces smaller than /, and torques smaller than 
r. Algorithm 3 contains the main steps of the transition 
algorithm, traj is the RRT output of the first stage, and 
p k represents a point in the composite trajectories of all the 
spacecraft at node k (4). To make the algorithms simpler, 


Algorithm 3 AuGMENT-TRAJECTORY(traj) 

1: augmented-traj <— 0 

2: for k < — 1 to SiZE(traj)— 1 do 

3: augmented-traj AWEND(augmented-traj , 

Propagate-DynamicsQA P k+1 )) 

4: end for 

5: augmented-traj Append (augmented-traj , 

Update-Node ( p end )) 

6: return augmented-traj 


it is assumed that the unknown values of the RRT output 
i.e., velocities, forces and torques, are all set to zero by 
default. An Update operation sets these variables to the 
correct values, and a Propagate operation creates new 
nodes using information of existing updated nodes. Lines 
2-4 in Algorithm 3 propagates the dynamics between each 
consecutive pairs of nodes of the trajectory. Line 5 ensures 
that the dynamics of the last node of the trajectory is 
also updated. Algorithm 4 describes how the dynamics are 


Algorithm 4 PROPAGATE-DYNAMICS(p cnrrent , p next ) 

1: <- GET-M AX-TlME(p cnrrent , p next ) 

2: for i <— 1 to TV do 

3: pcurrent _ UPDATE-NODE(^ nrrent , p ^ ex \ t max ) 

4: pinter p RO P-CURRENT-DYN(p^ nrrent , t max ) 

5: end for 

6: return jp current ? p mter } 


propagated between each pair of nodes. Line 1 calls Get- 
Max-Time to compute t max . Refer to Algorithm 5 for 
the steps involved in Get-Max-Time. Continuing with 
Algorithm 4, line 3 updates the forces and torques of the 
current node p current ? which are ensured to be feasible 
by construction. The velocities at p current remain zeros to 
satisfy the assumption of the first stage algorithm. Line 4 
propagates the dynamics to the intermediate node p inter by 
using the logic explained earlier in this section. 

Note that the transition step has to also ensure that the 
velocities of the spacecraft always lie between the permis- 
sible bounds. This check can be incorporated easily in the 
Get-Max-Time function, but it is not usually a dominating 
factor. The reason is that, in general, the main limitation of 
the spacecraft is the maximum thrust it can exert, not the 
maximum velocity it can reach. The transition step runs in 
O(Nri), where N is the size of the formation, and n is the 
total number of nodes in the RRT output. 

In summary, the transition step is a technique that aug- 
ments the output of the RRT output of the first stage of the 
RRT-GPM algorithm with feasible dynamics. It consists of 
adding intermediate nodes with a complete specific set of 
feasible dynamics, and synchronizes all spacecraft to get to 
each node at the same time with feasible forces and torques. 
The whole process can be easily automated. 



Algorithm 5 GET-MAX-TiME(p cnrrent , p next ) 

1 : tmax < 0 

2: for i 1 to TV do 

3: t maa; <- MAX(t ma;E , GET-MAX-ROTATION- 

TlME(p 2 current , ^ ext )) 

4: tmax <— MAX(£ ma;E , GET-MAX-TRANSLATION- 

TlME(p 2 current , p^)) 

5: end for 
6: return t ma;E 


C. The GPM Second Stage 

The second stage of the RRT-GPM algorithm formu- 
lates the multiple spacecraft reconfiguration maneuver as 
an optimal control problem. This optimal control problem 
is discretized at some specific discretization points called 
the Legendre-Gauss (LG) points, and then transcribed into 
a nonlinear program (NLP) by approximating the states 
and controls using Lagrange interpolating polynomials. The 
resulting NLP is then solved using the SNOPT nonlinear 
solver [44]. The augmented RRT output of the first stage is 
used as initial guess in solving the NLP, and is essential in 
1) reducing the computation times of the solution process, 
and 2) solving more complex reconfiguration problems. 

Pseudospectral methods have been a popular choice among 
numerical direct methods to solve optimal control problem 
due to their ability to provide accurate solutions of the 
costates and other covectors, without requiring the use of 
analytically differential equations of the adjoints [45]. An- 
other important feature of the pseudospectral methods is 
that they typically have faster convergence rate than other 
direct methods. They are known to demonstrate a “spectral 
accuracy” [46]. 

This paper uses a Gauss pseudospectral method, which 
has been shown to have, in general, more accurate solutions 
than other pseudospectral methods [38]. Another character- 
istic that distinguishes GPM among other pseudospectral 
methods is that the Karush-Kuhn-Tucker (KKT) conditions 
of the NLP have been shown to be exactly equivalent to 
the discretized form of the first-order optimality conditions 
of the Hamiltonian boundary value problem (HBVP) [47]. 
Therefore a solution to the NLP is guaranteed to satisfy the 
optimality conditions traditionally used in indirect methods, 
thus removing a primary disadvantage of direct methods. 
Ref. [38] and [47] are excellent references for pseudospectral 
methods, and more specifically for the Gauss pseudospectral 
method. 

The general formulation adopted in this second stage is the 
following [48], [47]. Determine the state, x(t), and control, 
u( t), that minimize the cost functional 

J = $(x(to),t 0 ,x(tf),tf) + f g(x(t),u(t),t)dt (25) 

Jt 0 


the boundary condition 

(f)(x(t 0 ),t 0 ,x(tf),tf) = 0 e R 9 (27) 

the inequality path constraints 

C{x(t), u(t), t) < 0 G M c (28) 

where to is the initial time, tf is the final time, and t G 

The optimal control problem of equations (25)-(28) is 
referred as the continuous Bolza problem. This problem is 
defined on [to, tf], where to and tf can be free or fixed 
variables. However, the Gauss pseudospectral method used 
to solve this problem requires a fixed time interval, such as 
[—1,1]. The mapping between the time interval t G [to, tf] 
and the time interval c G [—1,1] can be written as 

C = _ U+h (29) 

tf-to t f -to 

Rewrite the optimal control problem after replacing t with 

S': 


J = $(x(-l),t 0 ,x(l),tf) 

+ — 2 ~ / ^toi^to^o,*/)^ ( 3 °) 

subject to the constraints 

X = — - — F{ a(c), «(?), c; to, t f ) e R ra (31) 
4>(x(-l),t 0 ,x(l),tf) = 0 G R 9 (32) 

C(x(<;),u(<;),<;-,to,tf) < 0 e R c (33) 


where g G [—1,1]. (30)-(33) is called the transformed 
continuous Bolza problem. 

In the GPM, the set of A f discretization points includes 
JC = AT — 2 interior LG points, the initial point g = 0, 
and the final point s' = 1. GPM approximates the states by 
using a basis of JC+l Lagrange interpolating polynomials, 




i = 0 


where 


a«= n 




<^-Si 
Q - s j 


(34) 


(35) 


The control is approximated using a basis of 1C Lagrange 
interpolating polynomials C\, i = 1 . . . /C 




(36) 


i= 1 


where 


subject to the dynamic constraints 


4 to 


JC 


n 


£ - Si 
Ci — S j 


x = F(x(t ), u(t),t) G M n 


(26) 


(37) 



The dynamic constraints are transcribed into algebraic con- 
straints as follows 

Kl 

J2 D ki x i - t - l ^F(X(^ k ),U^ k ),^t 0 ,t f ) = 0 

i = 0 

(38) 


where k = 1 . . . /C, and D is an 1C x (JC + 1) differential 
approximation matrix, consisting of the derivative of each 
Lagrange polynomial corresponding to the state at each LG 
point. This matrix can be computed offline as follows: 


Du = £.(») = t (39) 

1=0 L Lj=0,j^i\^ ^3 / 


where k = 1 . . . JC and i = 0 . . . 1C. Note that the collocation 
of the dynamic constraint only happens at the LG points and 
not at the boundary points. Two additional variables, Xo 
and Xf are defined in this discretization. Xo = X(— 1), 
and X f is defined via a Gauss quadrature 


Xf = Xo + f ° y 2wkF(X(sk),U(sk),Sk;tp,tf) 

k= 1 

(40) 


where Wk are the Gauss weights. 

Continuing with the transcription process, (30) is approx- 
imated using a Gauss quadrature 


J = $(X(-l),t 0 ,X(l),tf) 
t -t K 

+ f „ ~ 'Y^w k g{X(<; k ),U{<; k ),<; k -,t 0 ,tf) (41) 

fc= 1 

The boundary constraint is written as 

4>(X(-l),to,X(l),t f ) = 0 (42) 

Finally, the path constraint is computed at the LG points as 

C(X(cfe),C/(cfc),ft;io,i/) < 0 (43) 

where k = 1 ...1C. Equations (38), (40), (41), (42) and 
(43) form an NLP that is the transcription of the modified 
continuous Bolza problem (MCBP). The solution of the NLP 
is an approximate solution to the MCBP. 


VI. Examples 

In this section, examples of different complexity are solved 
using the RRT-GPM technique. In the figures illustrating the 
examples, the trajectories are shown in solid lines, and each 
dot represents a time step. The spacecraft are shown on the 
trajectory every sixth to tenth time step, depending on the 
example. The plot axes represent the axes of the local iner- 
tially fixed frame. The vectors attached on each spacecraft 
are the X, Y, and Z body axes. Some examples also include 
some fixed obstacles, shown as green sphere-shaped objects. 
Furthermore, examples that have “stay outside” constraints 
show red ’’umbrellas”, with a handle showing the direction 
of the restricted pointing, and a cone of rays illustrating the 
angle of the constraint. Examples occasionally have circles 
around each spacecraft to explicitly show the boundaries 


of the spacecraft. The characteristics of the spacecraft are 
similar to those of SPHERES, and the dimensions of the test 
environment are similar to those of the SPHERES testbed on 
ISS [49]. Note that some of the examples are motivated by 
reconfiguration maneuvers described in Ref. [9]. 

A. Implementation Details 

The RRT first stage and the transition step are pro- 
grammed in C++, and they are compiled in Microsoft Visual 
Studio .NET 2003. The RRT first stage uses a linear solved 
based on the GLPK library. The GPM second stage is pro- 
grammed in Matlab, and uses the GPOCS software package 
[50]. GPOCS is a MATLAB implementation of the Gauss 
pseudospectral method for solving optimal control problems. 
GPOCS relies on SNOPT [44], an SQP solver for large-scale 
constrained optimization, to solve the NLP formed by the 
GPM method. The experiments are run on a Pentium 4, 2.2 
GHz processor equipped with 1GB of RAM. The number of 
discretization points used in the second stage is J\T = 25. 

B. Example: Two -Space craft Maneuver with Obstacle and 
Sun Avoidance 

The first example is a two- satellite maneuver that includes 
obstacle avoidance and sun avoidance constraint. Spacecraft 
1 and 2 are initially positioned at [0, 0, 0] T and [1, 1, 1] T . The 
maneuver consists of spacecraft 1 and 2 switching positions, 
and rotating 90° about the inertial Z-axis, while avoiding 
pointing at the sun and colliding with a fixed obstacle. The 
unit vector pointing at the sun is the vector [^, ^,0] T , 
surrounded by a cone of 25° half angle. The spacecraft 
must maintain its “sensitive” instrument ( e.g ., telescope lens), 
which is mounted in the direction of the body X axis, out 
of the cone. The fixed obstacle is a sphere centered at 
[0.3, 0.3, 0.3 ] t with a radius of 0.15 m. The computation 
times are 2 sec for the first stage, and 65 sec for the second 
stage. Figure 3 shows the output of first stage. The trajectory 
of both spacecraft are feasible, i.e., the spacecraft avoid 
colliding with the fixed obstacle, and do not point in the 
sun cone. However, the trajectories are clearly suboptimal. 
Figure 4 shows the final trajectory, after the RRT output is 
smoothed through the second stage. The spacecraft follow 
a trajectory that satisfies all the constraints, and minimizes 
energy consumption. 

C. Example: Coupled Two -Space craft Maneuver 

This example is a more complex two- spacecraft recon- 
figuration maneuver. Its complexity is due to the inclusion 
of inter- spacecraft pointing constraints, which add coupling 
between the position and attitude states of the spacecraft. 
In this example, spacecraft 1 and 2 are initially positioned 
at [0,0,0] T and [1,1,1] T . The maneuver consists of the 
spacecraft switching positions and rotating 180° about the 
inertial Z-axis, while satisfying an inter-spacecraft point- 
ing constraint and avoiding collisions with the two fixed 
obstacles. Note that both spacecraft must point their body 
X axis (solid blue) to the other spacecraft to within 32° . 
The centers of the obstacles are located at [0.5, 0.7, 0.5 ] T 
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Fig. 3. Example: Two-Spacecraft Maneuver with Obstacle and Sun 
Avoidance. RRT Output. Spacecraft 1 and 2 switch positions while avoiding 
colliding with a fixed obstacle and pointing to the sun. 




Fig. 5. Example: Two-Spacecraft Maneuver with Inter-Spacecraft Pointing 
and Obstacle Avoidance. RRT Output. Spacecraft 1 and 2 switch positions 
while avoiding colliding with two fixed obstacles and keep pointing to each 
other within 32°. 
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sun avoidance example. 

and [0.5, 0.1, 0.5] t , and have a radius of 0.15 m. The 
computation times are 4 sec for the first stage, and 112 sec 
for the second stage. 

Figure 5 shows the trajectory output after the RRT stage is 
completed. Notice that both spacecraft maintain their relative 
pointing, and avoid colliding with the obstacles. Spacecraft 
1 passes under the obstacle shown on the right of the 
figure, and Spacecraft 2 goes over the obstacle shown on 
the left. However, this strategy is clearly suboptimal, since 
there is enough space for both spacecraft to go in between 
the obstacles, and thus reduce fuel consumption. Figure 6 
shows the final trajectory produced by the second stage of 
RRT-GPM path planner. As expected, the trajectory of both 
spacecraft were moved towards the diagonal path, while 
maintaining feasibility of the constraints. 

This maneuver is designed to have a narrow passage 
in between the obstacles, which is usually hard for path 
planners to find. Figure 9 shows the inter- spacecraft collision 
avoidance constraint which is active for a large part of the 
maneuver, when the spacecraft enter the narrow passage 
between the obstacles. Figures 7 and 8 show the distance 
between the centers of spacecraft and the centers of each 
obstacle. The dashed lines show the minimum permissible 
distances. The figures illustrate how the spacecraft have 


Fig. 6. Final trajectory of the two- spacecraft maneuver with obstacle and 
inter- spacecraft pointing. 


to maneuver around the obstacles while avoiding colliding 
with them. Figure 10 shows the inter- spacecraft pointing 
constraints. Spacecraft 1 and 2 must keep their instrument, 
which is mounted on their body X axis, pointing at the 
other spacecraft to within 32°. Figure 10 shows that this 
is a restrictive constraint since it forces the instrument to 
be pointing within a thin cone during the entire maneuver. 
The inter- spacecraft pointing constraints always stay feasible 
except around t = 40 s and t = 80 s, but the deviations 
are within the feasibility tolerance specified in the GPOCS 
solver, and are thus acceptable. Finally, it is important to note 
that the first RRT stage is essential in enabling a solution to 
coupled maneuver problems similar to this example. In fact, 
the GPOCS solver failed to solve this problem every time it 
was not initialized with the RRT guess of the first stage. 

D. Example: Four-Spacecraft Maneuver 

This example is a more challenging reconfiguration ma- 
neuver. It involves four spacecraft with absolute pointing 
constraint and several inter- spacecraft constraints. So it is a 
highly coupled maneuver. Spacecraft 1 and 2 switch their 
positions and attitude while pointing their body X axis 
to each other within 33°. Spacecraft 1 and 2 are initially 
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Fig. 7. Distances between centers of the spacecraft and center of obstacle 
located at [0.5, 0.7, 0.5] T for the coupled two-spacecraft maneuver. 



Fig. 8. Distances between centers of the spacecraft and center of obstacle 
located at [0.5, 0.1, 0.5] T for the coupled two-spacecraft maneuver. 



Fig. 9. Distance between the centers of the spacecraft for the coupled 
two- spacecraft maneuver. The dashed line shows the minimum permissible 
distance 



Fig. 10. Cosines of angles between ranging device vector and relative 
position of both spacecraft for the coupled two-spacecraft maneuver. 
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Fig. 11. Example: Four-Spacecraft Maneuver with Inter- Spacecraft Point- 
ing and Absolute Pointing. RRT Output. Spacecraft 1 and 2 switch positions 
while keep pointing to each other within 33°. Spacecraft 3 and 4 keep both 
spacecraft 1 and 2 in their specified cone of 30° . 


located at [0,0.7, 0] T and [0,0, 0] T . Two other spacecraft 

3 and 4 are ’’health-monitoring” spacecraft 1 and 2. Both 
spacecraft 3 and 4 must end at their respective starting 
position, [0, —0.5.0] T and [0, 1.2, 0] T . They also have to keep 
pointing their body X axis at both spacecraft 1 and 2 to 
within 30° . All four spacecraft must also avoid pointing their 
X body axis in the sun cone. The sun cone is represented by 
the vector [1, 0, 0] T pointing at the sun and surrounded by a 
20° half angle cone. The computation times are 17 sec for 
the first stage, and 302 sec for the second stage. 

The trajectory produced by the RRT first stage is shown 
in Figure 1 1 . Notice that spacecraft 1 and 2 have to leave the 
X-Y plane in order to keep pointing to each other and avoid 
pointing in the sun direction. Consequently, spacecraft 3 and 

4 have to leave the X-Y plane in order to keep both spacecraft 
1 and 2 inside their respective pointing cones. This shows 
a clear coupling between the positions and attitudes of the 
spacecraft which is due to the absolute pointing and relative 
pointing constraints. All spacecraft also avoid colliding with 
each other. Figure 12 shows the final trajectory produced by 
the RRT-GPM planner. It is a smoothed version of Figure 1 1 . 
Figure 12 shows that all the constraints are met. Again, 
GPOCS failed to solve this problem when the RRT guess 
of the first stage was not given to it as an initial guess. This 
shows the importance of the first stage in enabling a solution 
for complex reconfiguration problems that include coupled 
constraints. 

The following section is another contribution of this 
paper. It underlines the importance of initializing GPM 
methods, and more generally pseudospectral methods, with 
an initial feasible guess. It reviews the different initializa- 
tion techniques that have been used for optimal control 
problems. Then it introduces an RRT technique as a way 
to initialize pseudospectral methods. Finally, it shows the 
improvement that the RRT initialization brings to spacecraft 
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Fig. 12. Final trajectory of the four- spacecraft maneuver with inter- 
spacecraft and absolute pointing. 

reconfiguration problems, which are solved using a Gauss 
pseudospectral method. 

VII. Initialization of Pseudospectral Methods 

Pseudospectral methods parameterize the states and con- 
trols of a continuous control problem using a basis of global 
polynomials, and then transcribes the discretized problem 
into an NLP The resulting NLP is solved using off-the-shelf 
nonlinear solvers. Traditionally, nonlinear solvers require a 
feasible initial point for their algorithms to converge to 
a local optimal solution. If this point is not provided by 
the user, the algorithm usually solves an auxiliary problem 
to compute it. Once this point is found, the algorithm 
either guarantees the feasibility of its iterates using a barrier 
function that keeps the iterates inside the boundary of the 
feasible set (e.g., interior point methods [51]), or it checks at 
every iteration to make sure the current point is in the feasible 
region, otherwise reestablishes feasibility (e.g., sequential 
quadratic programming methods [44]). 

Therefore, generating a “good” guess to the solver is 
definitely an important step towards reducing the number of 
iterations, and consequently the computation time, required 
to solve the NLP. A main motivation to put effort in reducing 
computation time is that it enables real-time path planning 
onboard spacecraft, thus reducing future space mission costs 
and increasing mission quality [52]. It is therefore highly 
desirable to be able to autonomously solve a path planning 
problem based on optimal control in real-time. Simplified 
versions of optimal control problems (e.g., linearized prob- 
lems) have been already solved in real-time control. How- 
ever, the main challenge is to implement nonlinear programs 
in real-time, and get consistent and reliable solutions [47]. 
Pseudospectral methods have the potential to be solved in 
real-time because they provide high accurate solutions even 
with a relatively small number of discretization nodes i.e., a 
smaller problem size [47]. Algorithms that can determine 
feasible initial guesses for pseudospectral methods will help 


reduce the computation time of these methods, and therefore 
make them closer for real-time applications. 

A. Survey of Initialization Techniques 

Note that generating a feasible initial guess to a highly 
constrained nonlinear problem can be as complicated as 
solving the nonlinear problem itself, so many researchers 
have suggested ways to compute good enough initial guesses. 
The closer the good guess to feasibility, the faster the solver 
will converge to the optimal solution. There have been 
different suggestions on how to generate a good guess for 
the nonlinear solver to solve the transcribed optimal control 
problem: 

1) One common way is that the user develops a good 
guess for the control using common-engineering-sense 
[53]. Then the states are computed by using numerical 
integration. The guess (states and controls) will most 
likely not be feasible, in the sense that it will not satisfy 
the boundary conditions. Then looking at the resulting 
guess and using knowledge of the problem, the user 
can create a new guess for the control, and so on, until 
the guess is good enough. But this process can be time 
consuming, and it is not guaranteed to converge to a 
feasible answer. 

2) Another method is known as the mesh refinement 
technique [26]. The idea is to start with a coarse grid 
(i.e., low number of discretization nodes) and use any 
guess (e.g., randomly chosen) as an initial starting point 
for the nonlinear solver. Then, if necessary, refine the 
discretization (i.e., increase the number of discretization 
nodes), and then repeat the optimization steps using 
the output of the previous step as the initial guess of 
the current step. But this approach can also be very 
time consuming, and therefore not feasible for online 
planning of reconfiguration maneuvers. 

3) A third way of initializing the NLP is by using a “warm” 
start approach [44], [54]. A warm start uses the output 
of a similar version of the NLP as the initial guess for 
the actual problem. A similar version is either exactly 
the same problem run previously (i.e., offline compared 
to online), or a simplified version of the problem. Warm 
starts are widely used for active set solvers, but there are 
still many difficulties in applying them to interior point 
methods [55]. In addition, using a warm start technique 
based on an offline computation is not suitable to online 
implementation where the environment is affected by 
disturbances, and where previous solutions might not 
therefore be even feasible. If a warm start process is to 
be efficient, the algorithm must be chosen with care. 

4) Something similar to a warm start is called homotopy 
methods that first solve a simpler version of the problem 
and then continuously modify the solution towards to 
the originally desired problem statement. 

The next section introduces a warm start technique based 
on a randomized planner, that computes an feasible initial 
feasible guess for a class of problems based on pseudospec- 
tral methods. This guess is the solution of a simplified 



Fig. 13. Steps Involved in Generating a Feasible Initial Guess to the 
Optimal Control Problem. This guess serves as a starting point to the 
NLP resulting from the transcription of the optimal control problem using 
Pseudospectral methods. 

version of a path planning problem without differential 
constraints. 

B. RRT Planner Initialization 

The main idea is to use an efficient warm start approach 
to initialize the NLP resulting from the transcription of 
the continuous optimal control problem. This warm start 
is formulated as an RRT path planning problem, with no 
differential constraints. Refer to Section III-B for more 
details about RRTs. The output of the RRT planner is 
then augmented with feasible dynamics that are propagated 
from source to destination using an algorithm that ensures 
feasibility at each node. This process of augmenting the 
output of the randomized planner is called the transition 
phase in this paper. The resulting output is a feasible initial 
guess to the complete optimal control problem. 

The paper uses an improved version of the bidirectional 
rapidly-exploring random trees (RRT) that is described in 
Ref. [21]. The transition phase is developed in Section V- 

B. The bidirectional RRT algorithm and the transition phase 
are adapted to solve the multiple spacecraft reconfiguration 
maneuver problem. The RRT algorithm and the transition 
phase can be adjusted to suit different optimal control 
problem specifics. This idea is best summarized in Figure 13. 

C. Illustration of the RRT improvement 

This section illustrates the improvement that the RRT 
initialization step brings to the solution of an optimal control 
problem transcribed into an NLP using a recently developed 
pseudospectral method method called the Gauss pseudospec- 
tral method (GPM) (Section V-C). This method has shown 
promise both in the accuracy of the solution and post- 
optimality analysis of optimal control problems [39]. The 
problem is a multispacecraft reconfiguration maneuver with 
path constraints that is described in Section IV. To underline 
the improvement of the RRT step, several reconfiguration 
maneuvers of increasing complexity are solved twice: 1) us- 
ing the RRT step to find a feasible initial guess i.e., following 
the two-stage approach described in Section V, and 2) using 
a “cold” start approach which leaves it to the nonlinear solver 
to find an initial starting guess. Refer to Section VI for details 
related to the figures illustrating reconfiguration maneuver 
examples. 


The examples consist of reconfiguration maneuvers that 
include stay outside constraints ( e.g ., sun avoidance), inter- 
spacecraft collision avoidance constraints, and obstacle 
avoidance constraints. These are hard non-convex con- 
straints. But the examples do not include inter- spacecraft 
constraints such as those found in the examples of Sec- 
tion VI-C and Section VI-D. The reason is that, when the 
cold approach was tried on those two examples, the nonlinear 
solver experienced numerical difficulties and failed to con- 
verge to a solution. Thus the RRT initial guess is essential 
in such examples. Recall that inter- spacecraft constraints are 
coupling constraints i.e., they affect, in a coupled way, the 
position and attitudes of the spacecraft. Thus, the feasible 
set of problems having coupled constraints is usually a very 
small region, which explains the difficulties of the nonlinear 
solver. The examples below are solved using GPOCS [50]. 
Both optimality and feasibility tolerances are set to 10 -4 . For 
more details about the implementation, refer to Section VI-A. 
The computation times and costs of the following examples 
are summarized in Table I and Table II. 

1 ) Single Spacecraft Maneuver: This example is a simple 
translation from the position [0,0,0] T to [1, 1, 1] T with a 
180° rotation around the Z axis. The spacecraft has to avoid 
pointing its X axis in the sun direction, which is represented 
by the vector [^, ^,0] T , surrounded by a cone of 30° half 
angle. It also has to avoid colliding with a fixed obstacle 
centered at [0.6, 0.5, 0.5] T , with radius equal to 0.15 m. 

2 ) Diagonally Crossing Maneuver: This problem consists 
of a three- spacecraft maneuver. Spacecraft 1 and 2 start at 
[0,0,0] T and [1,1,1] T , the opposite corners of a cube of 
side 1. They must switch position and make a 90° rotation 
around the inertial Z axis. A third spacecraft, spacecraft 3 
starts at[l, 0, 0] T and ends at [0, 1, 1] T . It also performs at 90° 
rotation. The maneuver of spacecraft three crosses diagonally 
those of the other two spacecraft. All three spacecraft have 
to avoid a fixed obstacle located at [0.3, 0.3, 0.3] T with 
radius 0.2 m. The same sun avoidance constraint described 
in Section VII-C.l applies in this problem. Figure 14 shows 
the RRT initial guess, the final trajectory of the problem that 
uses the RRT guess, and the final trajectory that does not 
use any initial guess. Two circles surrounding the spacecraft 
help visualize the boundaries of the spacecraft. 

3) Formation Reflection with Four Spacecraft: This exam- 
ple consists of four spacecraft that start in a square formation, 
and end in another reflected square formation. Spacecraft 1 
and spacecraft 3 start at [0, 1, 0] T and [2, 1, 0] T , respectively. 
They must end at their original positions. They must also 
rotate 90° around the inertial Z axis. Spacecraft 2 and 4 start 
at [1,0, 0] T and [1,2,0] T , and must switch their position. 
They must also rotate 180° around the inertial Z axis. All 
spacecraft must avoid pointing their X body axis inside two 
cones of 50° along the X and -X inertial directions. They 
must also avoid colliding with a fixed obstacle located at 
the center of the square, with radius 0.25 m. The pointing 
constraints lead to a non-trivial rotation maneuvers for both 
spacecraft 1 and 3. The fixed obstacle makes the trajectories 
of spacecraft 2 and 4 a more challenging one. In Summary, 
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(a) RRT Initial Guess 



(b) Final trajectory solved using RRT initial guess. 


(c) Final trajectory solved with a cold start approach. 

Fig. 14. RRT Path and Final Trajectories for the Three- Spacecraft Example 
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X body axis 

Y body axis 

Z body axis 


I I Satellite 3 




X body axis 

— — Y body axis 



(b) Final trajectory solved using RRT initial guess. 
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(c) Final trajectory solved with a cold start approach. 

Fig. 15. RRT Path and Final Trajectories for the Five- Spacecraft Example 


the maneuver is a reflection of a square formation around 
a line passing through the fixed positions of Spacecraft 1 
and 3. 

4) Formation Rotation with Five Spacecraft: This ex- 
ample consists of a five spacecraft starting in a pyramid 
formation. Spacecraft 5 starts at the apex of the pyramid, 
while spacecraft 1 to 4 form its square base. Each spacecraft 
must move to the next spacecraft position in the sequence 
(spacecraft 2 moves to spacecraft 1 position, 3 to 2, 4 to 3, 5 
to 4, and 1 to 5). Each spacecraft must also end the maneuver 
pointing in the direction where the next spacecraft in the se- 
quence was pointing at the beginning of the maneuver. Thus, 
the final configuration is a rotated version of the original 
pyramid formation. The spacecraft must avoid pointing their 
X body axis towards the sun direction represented by the 
vector [~j=, 4=, 0] T , surrounded by a cone of 20° half angle. 
They must also avoid colliding with two fixed obstacles of 
radius 0.15 m. Figure 15 shows the RRT initial guess, the 
final trajectory of the problem that uses the RRT guess, and 
the final trajectory that does not use any initial guess. 

D. Performance Comparison 

Table I summarizes the computation times of the examples 
used to show the improvement of the RRT initialization on 


TABLE I. Comparison of computation times of 

RECONFIGURATION MANEUVERS FOR FORMATION OF INCREASING SIZE 
SOLVED USING A GAUSS PSEUDOSPECTRAL METHOD (AVERAGE OVER 
10 RUNS) 



Time (s) 


Example 

w/o RRT 

with RRT 

Time Ratio 

1 s/c (VII-C.l) 

35 

24 

1.46 

2 s/c (VI-B) 

103 

67 

1.54 

3 s/c (VII-C.2) 

335 

171 

1.96 

4 s/c (VII-C.3) 

478 

228 

2.10 

5 s/c (VII-C.4) 

834 

356 

2.34 


problems solved using pseudospectral methods. The with 
RRT time includes the time of the RRT step, the transition 
step, and the time required by the GPOCS to solve the 
problem. The w/o RRT consists of the time needed by 
GPOCS to solve the same problems without an initial RRT 
guess i.e., using a cold start approach. The last column shows 
the ratio of the w/o RRT times over the with RRT times, 
i.e., the computation time improvement due to the RRT initial 
guess. 

Figure 16 displays side to side the computation times of 
both versions of the solutions of the reconfiguration maneu- 
vers as a function of the formation size. The results show that 



Fig. 16. Comparison of computation times of a series of reconfiguration 
maneuvers of increasing size solved using a Gauss pseudospectral method 
(average over 10 runs). The with RRT includes computation times of the 
RRT planner and the transition step. 



Fig. 17. Time Ratios of w/o RRT over with RRT Computation Times. The 
with RRT includes computation times of the RRT planner and the transition 
step. 

the RRT initialization reduces the GPOCS computation time, 
when compared to the w/o RRT case, by a factor increasing 
from 1.46 to 2.34 with the size of the formation. Figure 17 
illustrates this increase in time ratio. The time ratio values 
can be fit by a straight line with equation 

TimeRatio(N) = 0.237V + 1.18 (44) 

where N is the size of the formation. Thus, the time ratio of 
using the RRT initial guess is approximately linear for these 
formation sizes. 

It is also interesting to compare the computation scaling 
of each of the two cases. By linearly fitting the logarithms 
of the ratio time values versus the logarithms of the size of 
the formation, it can be computed that 

without RRT Time(N ) « 31.4V 2 01 (45) 

with RRT Time(N) « 23.3V 1 ' 69 (46) 

Thus, the w/o RRT computation time is approximately 
quadratic with the size of the formation, while the with 
RRT computation time is faster than linear, but slower than 
quadratic. Clearly, the RRT initial guess improves the scaling 
of the GPOCS computation time of the solution of the 
multiple spacecraft reconfiguration problem. 


TABLE II. Comparison of final costs of reconfiguration 

MANEUVERS FOR FORMATION OF INCREASING SIZE SOLVED USING A 

Gauss pseudospectral method (average over 10 runs) 



Cost 

Cost 

Example 

w/o RRT 

with RRT 

Comparison % 

1 s/c (VII-C.l) 

0.975 

1.012 

+3.65 

2 s/c (VI-B) 

1.806 

1.847 

+2.22 

3 s/c (VII-C.2) 

2.752 

2.821 

+2.45 

4 s/c (VII-C.3) 

4.490 

4.444 

-1.03 

5 s/c (VII-C.4) 

8.654 

8.743 

+ 1.02 


Table II summarizes the final costs of the same examples. 
The last column compares the cost of each example solved 
without an initial RRT guess to the cost of the same example 
solved with an RRT initial guess. A positive value indicates 
the percentage of improvement (i.e., decrease) in the cost of 
the w/o RRT case when compared to the with RRT one. One 
would expect that the w/o RRT costs to be better than those 
of the with RRT case. The reason is that, without an initial 
guess, the solver generates its own initial feasible guess that 
gives, on every run, the same best result it can find. The 
RRT guess, however, is based on a randomized planner, and 
therefore, can restrict the solver to solve the problem in a 
specific region, which might not be the best one. But, it can 
be seen from the cost results, that in average, the RRT guess 
did not have any noticeable impact on the final costs of the 
maneuvers. In the worst case, the cost of w/o RRT version 
is 3.65% better. For the largest formation, the five spacecraft 
reconfiguration described in Section VII-C.4, the w/o RRT 
cost is only 1.02% better. 

Finally, it should be emphasized that GPOCS fails to solve 
examples described in Sections VI-C and VI-D after 10 
attempts, when not initialized with an RRT initial guess. 
These examples include coupled constraints and they are 
therefore very challenging for the SNOPT nonlinear solver 
that GPOCS relies on in its solution process. However, 
when provided with an RRT guess, GPOCS is able to 
converge to an optimal solution. So not only does the RRT 
guess reduce the computation times of the reconfiguration 
maneuver problem, it also enables the solution of a com- 
plex class of reconfiguration problems that include inter- 
spacecraft constraints. 

VIII. Conclusions 

This paper presented a two-stage path planning technique 
for designing multiple spacecraft reconfiguration maneuvers 
with various path constraints. The primary idea was to com- 
bine an improved RRT planner with a Gauss pseudospectral 
method to obtain highly accurate solutions in computation 
times reasonable for online implementation. Several complex 
examples demonstrated the validity of this approach. This 
paper also showed the importance of an initialization step 
based on an RRT planner to the solution of problems solved 
using pseudospectral methods. It showed that this step 1) 
reduced the computation time of the solutions, and 2) made 
it possible to solve a more complex class of such problems. 
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